#include <XBee.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
#include "TimerThree.h"
#include <Wtv020sd16p.h>
#include <Tlc5940.h>

//Dylans Variables
XBee xbee = XBee();
AtCommandResponse rx64 = AtCommandResponse();
uint8_t fred[2] = {'D', 'B'};
AtCommandRequest req = AtCommandRequest(fred);
uint8_t* bob;
int sumvalue, i, distance, RxOne, RxTwo, RxTri;
boolean one, two, tri, fou, fiv, six, sev, eig;
unsigned long duration, duration2, duration3;

//Jacobs variables
boolean motion, scanning, attacking, waiting, watchdogWaiting, turning, pin3State;
byte incomingByte;
long time = 0;
const int led = 13;
int watchdog, previousState;
ZBRxResponse res = ZBRxResponse();

//Lucas' variables
// Creates the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 

// Select which 'port' M1, M2, M3 or M4. In this case, M3 and M1
Adafruit_DCMotor *myMotor = AFMS.getMotor(3);
Adafruit_DCMotor *myOtherMotor = AFMS.getMotor(1);

//Youchans variables
long detectingTime = 0;

//Bens variables
int resetPin = 22;
int clockPin = 28;
int dataPin = 26;
int busyPin = 24;
Wtv020sd16p wtv(resetPin, clockPin, dataPin, busyPin);

//Visual
int SIN = 51;
int SCLK = 52;
int XLAT = 11;
int BLANK = 12;
int GSCLK = 9;

void setup() {
    lucasSetup();
    benSetup();
    dylanSetup();
    youchanSetup();
    jacobSetup();
}

void loop() {
  if(watchdog != 3) {
    readState();
  }
  if(previousState != watchdog) {
    //state changed 
    disableInterrupt();
    attacking = false;
    waiting = false;
    watchdogWaiting = true;
    if(watchdog == 1) {
     initPwm();
     //initMotion();
    }
    else if(watchdog == 2) { //into follower
      greenOn();
      playAudio(1);
    }
  }
  else if(watchdog == 1) { //watchdog
    if(scanning) {
      if(!attacking && !waiting && !watchdogWaiting && !turning) { //do nothing if sounds are playing
        if(digitalRead(2) == HIGH) {
          pin3State = true; 
        }
        else {
          pin3State = false;
        }
        if(pin3State) { //attack
          disableInterrupt();
          waiting = true;
          attacking = false;
          initPwm();
        }
      }
    }
    else { //turn
      scanning = true;
      disableInterrupt();
      motorControl();
      attacking = false;
      initPwm(); 
    }
  }
  else if(watchdog == 2) { //follower 
    
    getDirection();
    if(one || two || tri || fou || fiv || six) {
      getDistance();
      motorControl();
    }
    //delay(1000);
  }
  previousState = watchdog;
  
}

void motorControl() {
    if(watchdog == 1) {
      turning = true;
        //disable motion sensor
        greenOn();
        myMotor->run(FORWARD);
        myOtherMotor->run(BACKWARD);
        myMotor->setSpeed(128);  
        myOtherMotor->setSpeed(128); 
        
        delay(266); //test to check 45 degrees
        myMotor->run(RELEASE);
        myOtherMotor->run(RELEASE);
        turning = false;
        delay(1500);
        blueOn();
        //enable motion sensor, set to zero
    }
    else if(watchdog == 2) { //follower mode
      greenOn();
      if(one)
      {
        //onward ho!
         //Serial1.println("FORWARD");
      }
      else if(two)
      {
        //60 degrees right
        myMotor->run(BACKWARD);
        myOtherMotor->run(FORWARD);
        myMotor->setSpeed(128);  
        myOtherMotor->setSpeed(128); 
        
        delay(355);
        myMotor->run(RELEASE);
        myOtherMotor->run(RELEASE);
      }
      else if(tri)
      {
        //120 degrees right
        myMotor->run(BACKWARD);
        myOtherMotor->run(FORWARD);
        myMotor->setSpeed(128);  
        myOtherMotor->setSpeed(128); 
        
        delay(710);
        myMotor->run(RELEASE);
        myOtherMotor->run(RELEASE);
      }
      if(fou)
      {
        //180 degrees right
        myMotor->run(BACKWARD);
        myOtherMotor->run(FORWARD);
        myMotor->setSpeed(128);  
        myOtherMotor->setSpeed(128); 
        
        delay(1065);
        myMotor->run(RELEASE);
        myOtherMotor->run(RELEASE);
      }
      else if(fiv)
      {
        //120 degrees left
        myMotor->run(FORWARD);
        myOtherMotor->run(BACKWARD);
        myMotor->setSpeed(128);  
        myOtherMotor->setSpeed(128); 
        
        delay(710);
        myMotor->run(RELEASE);
        myOtherMotor->run(RELEASE);
      }
      else if(six)
      {
        //60 degrees left
        myMotor->run(FORWARD);
        myOtherMotor->run(BACKWARD);
        myMotor->setSpeed(128);  
        myOtherMotor->setSpeed(128); 
        
        delay(355);
        myMotor->run(RELEASE);
        myOtherMotor->run(RELEASE);
      }
      
      //distance
      
      if(sumvalue > 42) { //5'
        //move 5'
        myMotor->run(BACKWARD);
        myOtherMotor->run(BACKWARD);
        myMotor->setSpeed(128);  
        myOtherMotor->setSpeed(128); 
        
        delay(9600); //test to check 45 degrees
        myMotor->run(RELEASE);
        myOtherMotor->run(RELEASE); 
      }
      else if(sumvalue > 39) { //4'
        //move 4'
        myMotor->run(BACKWARD);
        myOtherMotor->run(BACKWARD);
        myMotor->setSpeed(128);  
        myOtherMotor->setSpeed(128); 
        
        delay(7200); //test to check 45 degrees
        myMotor->run(RELEASE);
        myOtherMotor->run(RELEASE);         
      }
      else if(sumvalue > 36) { //3'
        //move 3'
        myMotor->run(BACKWARD);
        myOtherMotor->run(BACKWARD);
        myMotor->setSpeed(128);  
        myOtherMotor->setSpeed(128); 
        
        delay(1500); //test to check 45 degrees
        myMotor->run(RELEASE);
        myOtherMotor->run(RELEASE);        
      }
      else if(sumvalue > 33) { //2'
        //move 2'
        myMotor->run(BACKWARD);
        myOtherMotor->run(BACKWARD);
        myMotor->setSpeed(128);  
        myOtherMotor->setSpeed(128); 
        
        delay(4800); //test to check 45 degrees
        myMotor->run(RELEASE);
        myOtherMotor->run(RELEASE);        
      }
      else if(sumvalue > 30) { //1'
        //move 1'
        myMotor->run(BACKWARD);
        myOtherMotor->run(BACKWARD);
        myMotor->setSpeed(128);  
        myOtherMotor->setSpeed(128); 
        
        delay(2400); //test to check 45 degrees
        myMotor->run(RELEASE);
        myOtherMotor->run(RELEASE);        
      }
    }
}

// this plays the audio file with the name corresponding to the int parameter
void playAudio(int file){
  wtv.stopVoice();
  wtv.stopVoice();
  wtv.stopVoice();
  wtv.stopVoice();
  wtv.asyncPlayVoice(file);
  wtv.asyncPlayVoice(file);
  wtv.asyncPlayVoice(file);
  wtv.asyncPlayVoice(file);
}

// this function flashes specified led for duration intervals of 2sec, duration is length of blinking
void flashLed(int duration, int length, int redPin1, int redValue, int greenPin1, int greenValue, int bluePin1, int blueValue){
  int temp = duration, temp2 = 0, timeEnd = length;
  while(temp2 < timeEnd){
    Tlc.clear();
    Tlc.set(redPin1, redValue);
    Tlc.set(greenPin1, greenValue);
    Tlc.set(bluePin1, blueValue);
    Tlc.update();
    delay(duration);
    
    Tlc.clear();
    Tlc.update();
    delay(duration);
    
    temp2 += (temp * 2);
  } 
}

void readState() {
  xbee.readPacket(100);
  if(xbee.getResponse().isAvailable()) {
    if(xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
      xbee.getResponse().getZBRxResponse(res);
      uint8_t* words = res.getData();
      if(sizeof(words) > 0) {
        if(words[0] == 'W') {
          digitalWrite(led, HIGH);
          watchdog = 1;
        }
        else if(words[0] == 'F') {
          digitalWrite(led, LOW);
          watchdog = 2;
        }
        else {
          watchdog = 0;
        }
      }
    } 
  }
}

void locationControl() {
    //getDistance();
    getDirection();
    motorControl();
}

void getDirection() {
    one = false;
    two = false;
    tri = false;
    fou = false;
    fiv = false;
    six = false;
    
     // Reciever One Pin 31
    duration = pulseIn(31, LOW, 10000);
    duration2 = pulseIn(31, LOW, 10000);
    duration3 = pulseIn(31, LOW, 10000);
    if(duration > 2800 && duration < 3500)
    {
        //duration = pulseIn(31, LOW, 10000); //2500ish
      if(duration2 > 700 && duration2 < 1300)
      {
          //duration = pulseIn(31, LOW, 10000); //4500ish
        if(duration3 > 3600 && duration3 < 4400)
        {
            //Serial1.println("YOU FOUND ME One");
            one = true;
        }
      }
    }
    // Reciever Two Pin 33
    duration = pulseIn(33, LOW, 10000);
    duration2 = pulseIn(33, LOW, 10000);
    duration3 = pulseIn(33, LOW, 10000);
    if(duration > 2800 && duration < 3500)
    {
        //duration = pulseIn(33, LOW, 10000);
      if(duration2 > 700 && duration2 < 1300)
      {
          //duration = pulseIn(33, LOW, 10000);
        if(duration3 >3600 && duration3 < 4400)
        {
            //Serial1.println("YOU FOUND ME two");
            two = true;
        }
      }
    }  
    // Reciever Three Pin 35
    duration = pulseIn(35, LOW, 10000);
    duration2 = pulseIn(35, LOW, 10000);
    duration3 = pulseIn(35, LOW, 10000);
    if(duration > 2800 && duration < 3500)
    {
        duration = pulseIn(35, LOW, 10000);
      if(duration2 > 700 && duration2 < 1300)
      {
          duration = pulseIn(35, LOW, 10000);
        if(duration3 >3600 && duration3 < 4400)
        {
            //Serial1.println("YOU FOUND ME three");
            tri = true;
        }
      }
    }
     // Reciever One Pin 37
    duration = pulseIn(37, LOW, 10000);
    duration2 = pulseIn(37, LOW, 10000);
    duration3 = pulseIn(37, LOW, 10000);
    if(duration > 2800 && duration < 3500)
    {
        //duration = pulseIn(37, LOW, 10000); //2500ish
      if(duration2 > 700 && duration2 < 1300)
      {
          //duration = pulseIn(37, LOW, 10000); //4500ish
        if(duration3 >3600 && duration3 < 4400)
        {
            //Serial1.println("YOU FOUND ME One");
            fou = true;
        }
      }
    }
    // Reciever Two Pin 39
    duration = pulseIn(39, LOW, 10000);
    duration2 = pulseIn(39, LOW, 10000);
    duration3 = pulseIn(39, LOW, 10000);
    if(duration > 2800 && duration < 3500)
    {
        //duration = pulseIn(39, LOW, 10000);
      if(duration2 > 700 && duration2 < 1300)
      {
          //duration = pulseIn(39, LOW, 10000);
        if(duration3 >3600 && duration3 < 4400)
        {
            //Serial1.println("YOU FOUND ME two");
            fiv = true;
        }
      }
    }  
    // Reciever Three Pin 41
    duration = pulseIn(41, LOW, 10000);
    duration2 = pulseIn(41, LOW, 10000);
    duration3 = pulseIn(41, LOW, 10000);
    if(duration > 2800 && duration < 3500)
    {
        //duration = pulseIn(41, LOW, 10000);
      if(duration2 > 700 && duration2 < 1300)
      {
          //duration = pulseIn(41, LOW, 10000);
        if(duration3 >3600 && duration3 < 4400)
        {
            //Serial1.println("YOU FOUND ME three");
            six = true;
        }
      }
    }
}

void getDistance() {
    sumvalue = 0;
    for(i = 0;i<50;i++)
    {
      xbee.send(req);
      xbee.readPacket(100);
      if (xbee.getResponse().isAvailable())
      {
        if (xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE)
        {
            xbee.getResponse().getAtCommandResponse(rx64);
            bob = rx64.getValue();
            sumvalue = sumvalue + bob[0];
        }
      }
    }
    sumvalue = sumvalue / 50;
}

void countTime(){ //scanning
  detectingTime++;
  
  if(detectingTime >= 208333) // Once the executing time gets 3.75s, the program ends.
 {                  // 3.75second / 1 cycle(0.000018second) = 208333
                    // The interrupt is called each time the timer period finishes.
   scanning = false;
   disableInterrupt();
 }
}

void countTime2(){ //attacking
  detectingTime++;
  
  if(detectingTime >= 100000) // Once the executing time gets 3.75s, the program ends.
 {                  // 3.75second / 1 cycle(0.000018second) = 208333
                    // The interrupt is called each time the timer period finishes.
   disableInterrupt();
   attacking = false;
   blueOn();
   initPwm();
 }
}

void countTime3(){ //detected
  detectingTime++;
  
  if(detectingTime >= 150000) // Once the executing time gets 3.75s, the program ends.
 {                  // 3.75second / 1 cycle(0.000018second) = 208333
                    // The interrupt is called each time the timer period finishes.
   disableInterrupt();
   attacking = true;
   waiting = false;
   sendPacket();
   initPwm();
 }
}

void countTime4(){ //entered watchdog timer
  detectingTime++;
  
  if(detectingTime >= 288889) // Once the executing time gets 3.75s, the program ends.
 {                  // 3.75second / 1 cycle(0.000018second) = 208333
                    // The interrupt is called each time the timer period finishes.
   disableInterrupt();
   attacking = false;
   waiting = false;
   watchdogWaiting = false;
   scanning = true;
   initPwm();
 }
}

void countTime5(){ //startup
  detectingTime++;
  if(detectingTime >= 100000) // Once the executing time gets 3.75s, the program ends.
 {                  // 3.75second / 1 cycle(0.000018second) = 208333
                    // The interrupt is called each time the timer period finishes.
   disableInterrupt();
   watchdog = 0;
 }
}

void disableInterrupt() {
   Timer3.disablePwm(5);      //disable PWM
   Timer3.detachInterrupt();   //disable interrupt to count detecting time
   detectingTime = 0;  
   detachInterrupt(0);          //disable interrupt to get motion detected  
}

void sendPacket(){ // Send PWM signals to pin 5 depending on each envelop
  int count = 5; //  # of shooting packets at once
  
  //attack sounds
  Timer3.restart();
  
  while(count>=1){
  Timer3.restart();  
    
  Timer3.pwm(5, 256); // set pin 5 with 50% duty cycle
  delayMicroseconds(180); // keep sending for 180 micro seconds that is (10-cycle * 18us)
  Timer3.disablePwm(5); //disable PWM on pin 5
  delayMicroseconds(2520); // keep the pin as low during the rest of the cycle that is 150-10=140-cycle
  
  Timer3.pwm(5, 256); // Data Envelop, does same thing as above with different length of time
  delayMicroseconds(342);
  Timer3.disablePwm(5);
  delayMicroseconds(2358);
  
  Timer3.pwm(5, 256); // Redundant Envelop
  delayMicroseconds(342);
  Timer3.disablePwm(5);
  delayMicroseconds(2358);
  
  Timer3.pwm(5, 1024); // Stop Envelop, 100% duty cycle for stop envelop
  delayMicroseconds(2682);
  Timer3.disablePwm(5);
  delayMicroseconds(100000);
  
  count--;
  }
}

void greenOn() {
  Tlc.clear();
  Tlc.set(6, 4000);
  Tlc.set(3, 4000);
  Tlc.set(9, 4000);
  Tlc.set(12, 4000);
  Tlc.update();
}

void blueOn() {
  Tlc.clear();
  Tlc.set(4, 4000);
  Tlc.set(7, 4000);
  Tlc.set(10, 4000);
  Tlc.set(13, 4000);
  Tlc.update();
}

void redOn() {
  Tlc.clear();
  Tlc.set(2, 4000);
  Tlc.set(5, 4000);
  Tlc.set(8, 4000);
  Tlc.set(11, 4000);
  Tlc.update();
}

void yellowOn() {
  Tlc.clear();
  Tlc.set(2, 4000);
  Tlc.set(3, 4000);
  Tlc.set(5, 4000);
  Tlc.set(6, 4000);
  Tlc.set(8, 4000);
  Tlc.set(9, 4000);
  Tlc.set(11, 4000);
  Tlc.set(12, 4000);
  Tlc.update();
}

void ledOff() {
  Tlc.clear();
  Tlc.update();  
}

void lucasSetup() {
    AFMS.begin();  // create with the default frequency 1.6KHz
}

void dylanSetup() {
    Serial.begin(9600);       //Xbee
    xbee.setSerial(Serial);
    pinMode(led, OUTPUT);
    pinMode(31, INPUT); // Reciever One
    pinMode(33, INPUT); // Reciever Two
    pinMode(35, INPUT); // Reciever Three
    pinMode(37, INPUT); // Reciever Four
    pinMode(39, INPUT); // Reciever Five
    pinMode(41, INPUT); // Reciever Six
}

void jacobSetup() {
    watchdog = 0;
    previousState = 0;
    scanning = true;
    pinMode(led, OUTPUT);
    attacking = false; 
    waiting = false;
    watchdogWaiting = false;
    turning = false;
    pin3State = false;
}

void youchanSetup() {
    pinMode(3, OUTPUT);     // A pin to supply 5v to motion sensor
    digitalWrite(3, HIGH);  // This pin should be powered at the moment
                              // when the platform turns on to avoid 
                              //stabilizing time during actually detecting motion.
    pinMode(2, INPUT);
}

void benSetup() {
  Tlc.init(0);
  wtv.reset();
  pinMode(SCLK, OUTPUT);
  pinMode(SIN, OUTPUT);
  pinMode(XLAT, OUTPUT);
  pinMode(BLANK, OUTPUT);
  pinMode(GSCLK, OUTPUT);
  delay(1000);
  wtv.asyncPlayVoice(0xFFF9);
  wtv.asyncPlayVoice(0xFFF9);
  wtv.asyncPlayVoice(0xFFF9);
  playAudio(0);
  Timer3.attachInterrupt(countTime5);
  ledOff();
}

void initPwm(){ //Initialize PWM pin 5. Must be called once before calling damagePacket()
  detectingTime = 0;            //initialize detecting time
  
  Timer3.initialize(18);         // initialize timer3, and set a 18 micro second period
                                  // that equals to 56kHz
  if(attacking) {
    redOn();
    playAudio(4);
    Timer3.attachInterrupt(countTime2);
  }
  else if(waiting) { //motion found
    yellowOn();
    playAudio(3);
    Timer3.attachInterrupt(countTime3);
  }
  else if(watchdogWaiting) { //entered watchdog
    blueOn();
    playAudio(2);
    Timer3.attachInterrupt(countTime4);
  }
  else {  
    Timer3.attachInterrupt(countTime); //attach Interrupt on the timer to count time executing
                                    //this interrupts everytime the timer gets its end.(1 cycle)
  }
}

void initMotion()
{
  
    // Turn the input port on, that is connected to the output of motion sensor.
  
  attachInterrupt(0, sendPacket, RISING); // attach interrupt on pin 2
                                          // Interrupts when it is HIGH with detecting something
}
